#ifndef LOCAL_PLANNER_H
#define LOCAL_PLANNER_H

#include <mutex>
#include <ros/ros.h>
#include "sensor_msgs/PointCloud2.h"
#include "nav_msgs/Odometry.h"
#include <tf/transform_listener.h>
#include <pcl_ros/transforms.h>

#include "quad_msgs/ReplanCheck.h"
#include "local_planner/collision_checker.h"
#include "trajectory_optimization/trajectory_optimization.h"
#include "trajectory_optimization/bspline_smooth.h"

namespace Planner
{
    class LocalPlanner
    {
    public:
        LocalPlanner(const ros::NodeHandle &node_handle);
        ~LocalPlanner() = default;

        void init();

        void cloudMapCallback(const sensor_msgs::PointCloud2 &msgs);

        void odomCallback(const nav_msgs::Odometry &msgs);

        bool replanCheckCallback(quad_msgs::ReplanCheck::Request &request,
                                 quad_msgs::ReplanCheck::Response &response);

    private:
        ros::NodeHandle nh_;

        ros::Subscriber cloud_map_sub_;
        ros::Subscriber odom_sub_;

        ros::ServiceServer replan_server_;

        tf::TransformListener listener_;
        tf::StampedTransform transform_;
        int type_;

        double secure_dis_;
        double check_duration_; // 轨迹点安全性检验的时间间隔

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_ptr_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_ptr_;
        Vector3d curr_pose_;
        Quaterniond curr_orien_;

        CollisionChecker collision_checker_;
        TrajectoryOptimization traj_opt_;
        BsplineSmooth bspline_smooth_;

        std::mutex mutex_;

        // 对request中的轨迹进行离散采样和碰撞检测
        bool sampleWaypointsCheck(quad_msgs::ReplanCheck::Request &request, int start, int end,
                                  vector<int> &unsafe_traj_id, vector<Vector3d> &waypoints);
    };
} // namespace Planner

#endif